/**
 * @file RRTStarPlanner.h
 * @brief RRT*算法路径规划器
 * @details
 * RRT*算法原理：
 * 1. 基于快速探索随机树(RRT)的优化算法
 * 2. 通过重连接操作使路径渐进最优
 * 3. 适用于高维复杂环境
 *
 * 算法步骤：
 * 1. 初始化树，只包含根节点（起点）
 * 2. 循环直到达到最大迭代次数或找到路径：
 *    a. 在配置空间中随机采样
 *    b. 找到树上距离采样点最近的节点
 *    c. 从最近节点向采样点扩展一定距离，得到新节点
 *    d. 碰撞检测：检查扩展路径是否安全
 *    e. 寻找新节点附近的所有邻居节点
 *    f. 选择代价最小的节点作为父节点
 *    g. 将新节点加入树
 *    h. 重连接：检查新节点是否可以优化邻居节点的路径
 * 3. 从目标点回溯到起点，重建路径
 */

#ifndef RRTSTAR_PLANNER_H
#define RRTSTAR_PLANNER_H

#include "core/PlannerBase.h"
#include <vector>
#include <random>
#include <cmath>
#include <algorithm>
#include <memory>

namespace PathPlanning
{
/**
 * @brief RRT*算法配置参数
 */
struct RRTStarConfig : public PlannerConfig
{
    double stepSize      = 1.0;   // 扩展步长
    double goalBias      = 0.1;   // 目标偏向概率
    double searchRadius  = 2.0;   // 邻居搜索半径
    int    maxIterations = 10000; // 最大迭代次数

    // 优化参数
    bool   usePathOptimization   = true; // 是否使用路径优化
    double optimizationTolerance = 0.5;  // 优化容忍度

    RRTStarConfig()
    {
        plannerName = "RRTStar";
    }
};

/**
 * @brief RRT*树节点
 */
struct RRTStarNode : public Node, public std::enable_shared_from_this<RRTStarNode>
{
    std::vector<std::shared_ptr<RRTStarNode>> children;

    RRTStarNode(const Point& pos)
    {
        position = pos;
    }

    void addChild(std::shared_ptr<RRTStarNode> child)
    {
        children.push_back(child);
        child->parent = shared_from_this();
    }
};

class RRTStarPlanner : public PlannerBase
{
public:
    RRTStarPlanner(std::shared_ptr<Map> map);

    PlanningResult plan(const Point& start, const Point& goal) override;

private:
    // 随机数生成器
    std::random_device               rd_;
    std::mt19937                     gen_;
    std::uniform_real_distribution<> dis_;

    /**
     * @brief 在自由空间中随机采样
     */
    Point randomSample();

    /**
     * @brief 找到距离目标点最近的节点
     */
    std::shared_ptr<RRTStarNode> findNearestNode(const Point& target);

    /**
     * @brief 从最近节点向目标点扩展
     */
    Point steer(const Point& from, const Point& to, double maxDistance);

    /**
     * @brief 检查两点之间的路径是否无碰撞
     */
    bool isPathCollisionFree(const Point& from, const Point& to);

    /**
     * @brief 寻找新节点附近的邻居节点
     */
    std::vector<std::shared_ptr<RRTStarNode>> findNearNodes(const Point& point, double radius);

    /**
     * @brief 选择最优父节点
     */
    std::shared_ptr<RRTStarNode> chooseBestParent(const std::vector<std::shared_ptr<RRTStarNode>>& nearNodes,
                                                  const Point&                                     newPoint);

    /**
     * @brief 重连接操作：优化树结构
     */
    void rewire(std::shared_ptr<RRTStarNode> newNode, const std::vector<std::shared_ptr<RRTStarNode>>& nearNodes);

    /**
     * @brief 检查是否到达目标
     */
    bool isGoalReached(const Point& point, const Point& goal);

    /**
     * @brief 从目标节点重建路径
     */
    std::vector<Pose> reconstructPath(std::shared_ptr<RRTStarNode> goalNode);

    /**
     * @brief 路径优化：缩短路径
     */
    std::vector<Pose> optimizePath(const std::vector<Pose>& path);

    // RRT*树
    std::shared_ptr<RRTStarNode>              root_;
    std::vector<std::shared_ptr<RRTStarNode>> nodes_;
};

} // namespace PathPlanning

#endif